import open3d as o3d
import numpy as np


def getCuboidPoints(obb):
  # return np.array([
  #   # Vertices Polygon1
  #   [obb.center[0] + (obb.extent[0] / 2), obb.center[1] + (obb.extent[1] / 2), obb.center[2] + obb.extent[2]], # face-topright
  #   [obb.center[0]  - (obb.extent[0] / 2), obb.center[1] + (obb.extent[1] / 2), obb.center[2] + obb.extent[2]], # face-topleft
  #   [obb.center[0] - (obb.extent[0] / 2), obb.center[1] - (obb.extent[1] / 2), obb.center[2] + obb.extent[2]], # rear-topleft
  #   [obb.center[0] + (obb.extent[0] / 2), obb.center[1] - (obb.extent[1] / 2), obb.center[2] + obb.extent[2]], # rear-topright
  #
  #   # Vertices Polygon 2
  #   [obb.center[0] + (obb.extent[0] / 2), obb.center[1] + (obb.extent[1] / 2), obb.center[2] - obb.extent[2]],
  #   [obb.center[0] - (obb.extent[0] / 2), obb.center[1] + (obb.extent[1] / 2), obb.center[2] - obb.extent[2]],
  #   [obb.center[0] - (obb.extent[0] / 2), obb.center[1] - (obb.extent[1] / 2), obb.center[2] - obb.extent[2]],
  #   [obb.center[0] + (obb.extent[0] / 2), obb.center[1] - (obb.extent[1] / 2), obb.center[2] - obb.extent[2]],
  # ]).astype("float64")
  return np.array([
      # Vertices Polygon1
      [obb.center[0] + (obb.extent[0] ), obb.center[1] + (obb.extent[1] ), obb.center[2] + obb.extent[2]],
      # face-topright
      [obb.center[0] - (obb.extent[0]), obb.center[1] + (obb.extent[1]), obb.center[2] + obb.extent[2]],
      # face-topleft
      [obb.center[0] - (obb.extent[0]), obb.center[1] - (obb.extent[1]), obb.center[2] + obb.extent[2]],
      # rear-topleft
      [obb.center[0] + (obb.extent[0]), obb.center[1] - (obb.extent[1]), obb.center[2] + obb.extent[2]],
      # rear-topright
    
      # Vertices Polygon 2
      [obb.center[0] + (obb.extent[0]), obb.center[1] + (obb.extent[1]), obb.center[2] - obb.extent[2]],
      [obb.center[0] - (obb.extent[0]), obb.center[1] + (obb.extent[1]), obb.center[2] - obb.extent[2]],
      [obb.center[0] - (obb.extent[0] ), obb.center[1] - (obb.extent[1]), obb.center[2] - obb.extent[2]],
      [obb.center[0] + (obb.extent[0]), obb.center[1] - (obb.extent[1]), obb.center[2] - obb.extent[2]],
  ]).astype("float64")

def crop_pc(downpcd):
    downpcd.estimate_normals(
        search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
    aabb = chair.get_axis_aligned_bounding_box()
    oriented_bounding_box = o3d.geometry.OrientedBoundingBox.create_from_axis_aligned_bounding_box(aabb)
    point_cloud_crop = downpcd.crop(oriented_bounding_box)
    return point_cloud_crop
if __name__=='__main__':
    import copy
    pcd = o3d.io.read_point_cloud('/home/wsl/zz/replay_93936_2021-9-22.ply')
    downpcd = pcd.voxel_down_sample(voxel_size=0.1)
    downpcd.estimate_normals(
        search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
    chair=copy.deepcopy(downpcd)
    aabb = chair.get_axis_aligned_bounding_box()
    aabb.color = (1, 0, 0)
    obb = chair.get_oriented_bounding_box()
    cuboid_points = getCuboidPoints(obb)

    points = o3d.utility.Vector3dVector(cuboid_points)
    #oriented_bounding_box = o3d.geometry.OrientedBoundingBox.create_from_points(points)
    oriented_bounding_box=o3d.geometry.OrientedBoundingBox.create_from_axis_aligned_bounding_box(aabb)
    point_cloud_crop = downpcd.crop(oriented_bounding_box)
    point_cloud_crop.paint_uniform_color([0.0,1.0,0])
    chair.paint_uniform_color([1.0, 0.0, 0])
    obb.color = (0, 1, 1)
    # # o3d.visualization.draw_geometries([croped],
    # #                                   zoom=0.7,
    # #                                   front=[0.5439, -0.2333, -0.8060],
    # #                                   lookat=[2.4615, 2.1331, 1.338],
    # #                                   up=[-0.1781, -0.9708, 0.1608])
    o3d.visualization.draw_geometries([point_cloud_crop, obb],
                                      zoom=0.7,
                                      front=[0.5439, -0.2333, -0.8060],
                                      lookat=[2.4615, 2.1331, 1.338],
                                      up=[-0.1781, -0.9708, 0.1608])
    # pcd.normals = o3d.utility.Vector3dVector(np.zeros(
    #     (1, 3)))  # invalidate existing normals
    #
    # pcd.estimate_normals()
    # #o3d.visualization.draw_geometries([pcd], point_show_normal=True)
    # print('run Poisson surface reconstruction')
    # with o3d.utility.VerbosityContextManager(
    #         o3d.utility.VerbosityLevel.Debug) as cm:
    #     mesh, densities = o3d.geometry.TriangleMesh.create_from_point_cloud_poisson(
    #         pcd, depth=9)
    # print(mesh)
    # o3d.visualization.draw_geometries([mesh],
    #                                   zoom=0.664,
    #                                   front=[-0.4761, -0.4698, -0.7434],
    #                                   lookat=[1.8900, 3.2596, 0.9284],
    #                                   up=[0.2304, -0.8825, 0.4101])